#include <driver/hardware_interface.h>
#include <driver/driver.h>
#include <ros/callback_queue.h>
#include <thread>
#include <agv_msgs/msg/output_port.hpp>
#include <agv_srvs/srv/output_interface.hpp>
#include <agv_msgs/msg/read_in_puts.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_msgs/msg/pair_type_int.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <driver/controller_manager.h>
 
std::shared_ptr<ros_canopen_driver::HardwareInterface> hardware_interface_;
std::shared_ptr<resource_manager::ControllerManager>   controller_manager_;

int reset_level = 0;
bool canopen_ready = false;
bool driver_initialized = false;

bool handle_init(std_srvs::srv::Trigger::Request &req, std_srvs::srv::Trigger::Response &res)
{
    RCLCPP_INFO(rclcpp::get_logger("Driver"), "Initializing XXX");
    //to do init
    res.success = hardware_interface_->servoInit();
    res.message = "";
    return true;
}

bool handle_recover(std_srvs::srv::Trigger::Request &req, std_srvs::srv::Trigger::Response &res)
{
    if (driver_initialized) {
        RCLCPP_INFO(rclcpp::get_logger("Driver"), "Recover servo");
        //to do recover
        res.success = hardware_interface_->servoRecover();
        res.message = "";
        return true;
    }
    else {
        res.success = true;
        return true;
    }
}

bool handle_shutdown(std_srvs::srv::Trigger::Request &req, std_srvs::srv::Trigger::Response &res)
{
    if (driver_initialized) {
        RCLCPP_INFO(rclcpp::get_logger("Driver"), "Shutdown XXX");
        //to do shutdown
        res.success = hardware_interface_->servoShutdown();
        res.message = "";
        return true;
    }
    else {
        res.success = true;
        return true;
    }
}

bool handle_halt(std_srvs::srv::Trigger::Request &req, std_srvs::srv::Trigger::Response &res)
{
    if (driver_initialized) {
        RCLCPP_INFO(rclcpp::get_logger("Driver"), "Halt XXX");
        //to do halt
        res.success = hardware_interface_->servoHalt();
        res.message = "";
        return true;    
    }
    else {
        res.success = true;
        return true;
    }
    
}

void reset_button_callback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr& msg)
{
    // int new_reset_level = (msg->ret_bits >> 1) & 0x1;
    int new_reset_level = 0;

    std::vector<agv_msgs::msg::PairTypeInt> input_msg = msg->array;

    for (const auto &data : input_msg) {
        if (data.key.compare(0, 15, "emergencyButton") == 0) {
            if (data.value == 1) {
                return;
            }
        }
    }

    for (const auto &data : input_msg) {
        if (data.key.compare("resetButton") == 0) {
            new_reset_level = data.value;
        }
    }

    if (new_reset_level != reset_level && new_reset_level == 1) {
        hardware_interface_->ep_.clear();
        // if (hardware_interface_->hasError() && driver_initialized) {
        //     hardware_interface_->servoRecover();
        // }
        hardware_interface_->callNmtService(1);
    }

    reset_level = new_reset_level;
}

// void canopen_ready_callback(const std_msgs::msg::Bool::ConstSharedPtr& msg)
// {
//     canopen_ready = msg->data;
// }


int main(int argc, char **argv)
{
    /*
    if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) {
        ros::console::notifyLoggerLevelsChanged();
    }
    */
	rclcpp::init(argc, argv);
    auto nh_ = rclcpp::Node::make_shared("driver");
    // set 8 thread to handle kacanopen topics
    ros::AsyncSpinner spinner_(12);
    spinner_.start();

    ros::ServiceServer srv_init_ = nh_.advertiseService("driver/init", handle_init);
    ros::ServiceServer srv_recover_ = nh_.advertiseService("driver/recover", handle_recover);
    ros::ServiceServer srv_halt_ = nh_.advertiseService("driver/halt", handle_halt);
    ros::ServiceServer srv_shutdown_ = nh_.advertiseService("driver/shutdown", handle_shutdown);
    auto readinput_sub = nh_.subscribe("/readInputNew", 10, &reset_button_callback);

    hardware_interface_.reset(
			new ros_canopen_driver::HardwareInterface(nh_));
    controller_manager_.reset(
            new resource_manager::ControllerManager(nh_, hardware_interface_.get()));

    if (!controller_manager_->init()) {
        RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[driver] controller manager init failed.");
        spinner_.stop();
        ros::waitForShutdown();
        return 0;
    }

    // controller_manager_->start();

    
    boost::chrono::system_clock::time_point begin_ = boost::chrono::system_clock::now();
    boost::chrono::system_clock::time_point end_   = boost::chrono::system_clock::now();
    boost::chrono::system_clock::time_point last_io_time_;

    // rclcpp::Time internal_time_(0);
    const rclcpp::Duration dt_ = hardware_interface_->getPeriod();
    double elapsed_secs_ = 0;

    driver_initialized = false;

    
    ros::ServiceClient output_client= nh_.serviceClient<agv_srvs::srv::OutputInterface>("/outputinterface");
    std::vector<agv_msgs::msg::OutputPort> outports;
    agv_msgs::msg::OutputPort servoIO;
    agv_srvs::srv::OutputInterface srv;

    outports.clear();
    outports.resize(0);

    int servo_power_io_index = -1;
    nh_.param<int>("/IO/output_device/servoPower", servo_power_io_index, -1);

    if (servo_power_io_index >= 0) {
        servoIO.index = servo_power_io_index;
        servoIO.level = true;
        outports.push_back(servoIO);
    }

    int brake_out_index = -1;
    nh_.param<int>("/IO/output_device/brakeOut", brake_out_index, -1);
    agv_msgs::msg::OutputPort brakeIO;

    if (brake_out_index >= 0) {
        brakeIO.index = brake_out_index;
        brakeIO.level = true;
        outports.push_back(brakeIO);
    }

    int jacking_power_io_index = -1;
    nh_.param<int>("/IO/output_device/liftRelay", jacking_power_io_index, -1);
    agv_msgs::msg::OutputPort jackingIO;;

    if (jacking_power_io_index >= 0) {
        jackingIO.index = jacking_power_io_index;
        jackingIO.level = true;
        outports.push_back(jackingIO);
    }

    int pre_charge_io_index = -1;
    nh_.param<int>("/IO/output_device/preChargeOff", pre_charge_io_index, -1);
    agv_msgs::msg::OutputPort pre_charge_io;

    if (pre_charge_io_index >= 0) {
        pre_charge_io.index = pre_charge_io_index;
        pre_charge_io.level = true;
        outports.push_back(pre_charge_io);
    }

    srv.request.index_level = outports;

    while (rclcpp::ok())
    {
        if (!driver_initialized) {
            if (outports.size() > 0) {
                if (output_client.call(srv) && srv.response.state) {
                    last_io_time_ = boost::chrono::system_clock::now();
                    RCLCPP_INFO(rclcpp::get_logger("Driver"), "[Driver] set servo_power/brakeOut/jacking_power output IO succeed");
                }
                else {
                    RCLCPP_INFO(rclcpp::get_logger("Driver"), "[Driver] set servo_power/brakeOut/jacking_power output IO failed");
                    rclcpp::Duration(1).sleep();
                    continue;
                } 
            }

            driver_initialized = true;
            if (!driver_initialized) {
                rclcpp::Duration(1).sleep();
            }
        }
        else {
            begin_ = boost::chrono::system_clock::now();

            if (outports.size() > 0) {
                if (boost::chrono::duration_cast<boost::chrono::duration<double> >((begin_ - last_io_time_)).count() > 3.0) {
                    if (output_client.call(srv) && srv.response.state) {
                        last_io_time_ = begin_;
                        RCLCPP_INFO(rclcpp::get_logger("Driver"), "[Driver] set servo_power/brakeOut/jacking_power output IO succeed");
                    }
                    else {
                        RCLCPP_ERROR(rclcpp::get_logger("Driver"), "[Driver] set servo_power/brakeOut/jacking_power output IO failed");
                    } 
                }
            }

            // Basic bookkeeping to get the system time in order to compute the control period.
            hardware_interface_->read();
            controller_manager_->update(/*internal_time_*/rclcpp::Time::now(), dt_);
            hardware_interface_->write();

            end_ = boost::chrono::system_clock::now();

            elapsed_secs_ = boost::chrono::duration_cast<boost::chrono::duration<double> >((end_ - begin_)).count();
            
            if (dt_.toSec() - elapsed_secs_ < 0.0)
            {
              ROS_WARN_STREAM_THROTTLE(
                    0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs_);
            }
            else
            {
              usleep((dt_.toSec() - elapsed_secs_) * 1e6);
            }
            // internal_time_ += dt_;
        }
    }
   

    if (hardware_interface_ != nullptr)
        hardware_interface_->quit();

    spinner_.stop();
    ros::waitForShutdown();
    return 0;
}
